# coding : utf-8
"""
测试carla的一些功能
"""
import argparse
import collections
import datetime
import glob
import logging
import math
import os
import random
import re
import sys
import weakref

try:
    import numpy as np
except ImportError:
    raise RuntimeError(
        'cannot import numpy, make sure numpy package is installed')

# ==============================================================================
# -- Find CARLA module ---------------------------------------------------------
# ==============================================================================
try:
    carla_dir = glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]
    print('carla_dir=', carla_dir)
    sys.path.append(carla_dir)
except IndexError:
    print('carla not found')
    pass

# ==============================================================================
# -- Add PythonAPI for release mode --------------------------------------------
# ==============================================================================
try:
    sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla')
except IndexError:
    pass

import carla
import time

from agents.tools.misc import spawn_vehilce_at_point, draw_speed_with_tf, set_bird_view
from agents.tools.vehicle import MotionStatus

from agents.map.local_map import LocalRoadTreeDeque, get_transform_by_bias
#
if __name__ == "__main__":
    try:
        client = carla.Client('localhost', 2000)
        client.set_timeout(4.0) 

        client.reload_world()
        world = client.get_world()
        car_bp = world.get_blueprint_library().filter("*vehicle")[0] # CarlaOk
        car = spawn_vehilce_at_point(world, carla.Location(0,0,0), car_bp)

        spectator = world.get_spectator()
        transform = car.get_transform()
        spectator.set_transform(carla.Transform(transform.location + carla.Location(z=50),
        carla.Rotation(pitch=-90)))

        car.set_autopilot()

        my_map = world.get_map()
        waypoint = my_map.get_waypoint(car.get_location())
        print(waypoint)

        # obstacle = spawn_vehilce_at_point(world, carla.Location(10,10,0), car_bp)

        hero_tf = car.get_transform()
        # obs_tf = obstacle.get_transform()

        # print('try sub vector3d')
        # diff_loc = hero_tf.location - obs_tf.location
        # print('result is ', diff_loc)

        hero_speed = car.get_velocity()
        # obs_speed = obstacle.get_velocity()
        # print('try sub speed')
        # diff_speed = hero_speed - obs_speed
        # print('result is ', diff_speed.x)

        while True:
            car_transform = car.get_transform()
            debug0_tf = get_transform_by_bias(car_transform, 3.5)
            draw_speed_with_tf(world, debug0_tf, life_time=4.0)
            debug1_tf = get_transform_by_bias(car_transform, -3.5)
            draw_speed_with_tf(world, debug1_tf, color=carla.Color(0,0,255), life_time=4.0)

            set_bird_view(spectator, car)
            time.sleep(5)
            car_status = MotionStatus(car, 30.0, dt=0.01)
            car_status.next_status()
            print('next_status=', car_status._location, car_status._yaw)

    finally:
        if car is not None:
            car.destroy()